//----------------------------------------------------------------------------------------------
//	Filename:	SegmentsGraph.h
//	Author:		Sandro Spina
//	Date:		27/01/2013
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#pragma once
#include "UniqueId.h"
#include <map>
#include <Geometry\Vector3.h>
#include <Geometry\Plane.h>

namespace Illumina
{
	namespace Core
	{
		class PointCloudSegmentation;
		class StructureGraph;
		class SegmentsGraph
		{
		public:
				
			::UniqueID * segmentID;

			enum Shape {line,plane,sphere,compound,unknown};

			PointCloudSegmentation * m_pcs;			

			struct EdgeSegment;

			struct OBB {	
				Vector3 obb_0;		//u-axis
				Vector3 obb_1;		//v-axis
				Vector3 obb_2;		//w-axis
				Plane p_0_min;		//from first eigenvalue (smallest extent)
				Plane p_0_max; 
				Plane p_1_min;		//from second eignevalue (middle extent)
				Plane p_1_max; 
				Plane p_2_min;		//from third eigenvalue (largest extent)
				Plane p_2_max; 
				float buffer;			   

				int sizeInBytes()
				{
					int s = 0;
					s += sizeof(float) * 3 * 3;  //X,Y,Z floats
					s += sizeof(float) * 4 * 6;  //X,Y,Z,Dst floats
					s += sizeof(float); // OBB buffer
					return s;
				}

				std::vector<Vector3> GetOBBCorners()
				{
					std::vector<Vector3> corners;
					corners.push_back(Vector3(obb_0.X*p_0_min.Distance + obb_1.X*p_1_min.Distance + obb_2.X*p_2_min.Distance, obb_0.Y*p_0_min.Distance + obb_1.Y*p_1_min.Distance + obb_2.Y*p_2_min.Distance, obb_0.Z*p_0_min.Distance + obb_1.Z*p_1_min.Distance + obb_2.Z*p_2_min.Distance));  
					corners.push_back(Vector3(obb_0.X*p_0_min.Distance + obb_1.X*p_1_max.Distance + obb_2.X*p_2_min.Distance, obb_0.Y*p_0_min.Distance + obb_1.Y*p_1_max.Distance + obb_2.Y*p_2_min.Distance, obb_0.Z*p_0_min.Distance + obb_1.Z*p_1_max.Distance + obb_2.Z*p_2_min.Distance)); 
					corners.push_back(Vector3(obb_0.X*p_0_min.Distance + obb_1.X*p_1_max.Distance + obb_2.X*p_2_max.Distance, obb_0.Y*p_0_min.Distance + obb_1.Y*p_1_max.Distance + obb_2.Y*p_2_max.Distance, obb_0.Z*p_0_min.Distance + obb_1.Z*p_1_max.Distance + obb_2.Z*p_2_max.Distance));  
					corners.push_back(Vector3(obb_0.X*p_0_min.Distance + obb_1.X*p_1_min.Distance + obb_2.X*p_2_max.Distance, obb_0.Y*p_0_min.Distance + obb_1.Y*p_1_min.Distance + obb_2.Y*p_2_max.Distance, obb_0.Z*p_0_min.Distance + obb_1.Z*p_1_min.Distance + obb_2.Z*p_2_max.Distance));
					corners.push_back(Vector3(obb_0.X*p_0_max.Distance + obb_1.X*p_1_min.Distance + obb_2.X*p_2_min.Distance, obb_0.Y*p_0_max.Distance + obb_1.Y*p_1_min.Distance + obb_2.Y*p_2_min.Distance, obb_0.Z*p_0_max.Distance + obb_1.Z*p_1_min.Distance + obb_2.Z*p_2_min.Distance));  
					corners.push_back(Vector3(obb_0.X*p_0_max.Distance + obb_1.X*p_1_max.Distance + obb_2.X*p_2_min.Distance, obb_0.Y*p_0_max.Distance + obb_1.Y*p_1_max.Distance + obb_2.Y*p_2_min.Distance, obb_0.Z*p_0_max.Distance + obb_1.Z*p_1_max.Distance + obb_2.Z*p_2_min.Distance));  
					corners.push_back(Vector3(obb_0.X*p_0_max.Distance + obb_1.X*p_1_max.Distance + obb_2.X*p_2_max.Distance, obb_0.Y*p_0_max.Distance + obb_1.Y*p_1_max.Distance + obb_2.Y*p_2_max.Distance, obb_0.Z*p_0_max.Distance + obb_1.Z*p_1_max.Distance + obb_2.Z*p_2_max.Distance));  
					corners.push_back(Vector3(obb_0.X*p_0_max.Distance + obb_1.X*p_1_min.Distance + obb_2.X*p_2_max.Distance, obb_0.Y*p_0_max.Distance + obb_1.Y*p_1_min.Distance + obb_2.Y*p_2_max.Distance, obb_0.Z*p_0_max.Distance + obb_1.Z*p_1_min.Distance + obb_2.Z*p_2_max.Distance));  
					return corners;
				}

				float GetVolume()
				{
					return (p_0_max.Distance - p_0_min.Distance) * (p_1_max.Distance - p_1_min.Distance) * (p_2_max.Distance - p_2_min.Distance);
				}
			};

			struct SurfaceSegment
			{
				int id;
				std::string label;
				std::vector<int> * points;
				Shape shape_primitive;
				Illumina::Core::Vector3 bounding_min;
				Illumina::Core::Vector3 bounding_max;
				Illumina::Core::Vector3 orientation;
				std::map<int, EdgeSegment*> * connections;
				SegmentsGraph * internalSegGraph;
				StructureGraph * internalStrGraph;
				int internalGraphSelectionIndex;
				int visibilityCount;
				int maxDecompositionCount; //Stores the maximum number of surface parts this surface was decomposed in (from a specific view)
				float coverage;
				OBB obb;
				float plane_d;

				int sizeInBytes()
				{
					int s = 0;
					s += sizeof(id);					
					s += sizeof(int) + (points->size() * sizeof(int));
					s += sizeof(shape_primitive);
					s += sizeof(float) * 3;
					s += sizeof(float) * 3;
					s += sizeof(float) * 3;
					s += sizeof(float); //Add coverage
					s += sizeof(int) + (connections->size() * sizeof(int));
					s += sizeof(int); //if internal seg graph exists then 1, otherwise 0 (internal graph == NULL)					
					s += sizeof(int);  //internal graph selection index
					s += sizeof(int);  //visibilityCount
					s += sizeof(int);  //maxDecompositionCount
					s += obb.sizeInBytes();   //size of OBB	
					return s;
				}

				bool isCoplanar(float _diff, SurfaceSegment * _with) {							
					float dot = Vector3::Dot(this->obb.obb_0, _with->obb.obb_0);
					if (Maths::Abs(dot) > 0.95f)
					{
						float d1 = this->obb.p_0_min.Distance + ((this->obb.p_0_max.Distance - this->obb.p_0_min.Distance) / 2);
						float d2 = _with->obb.p_0_min.Distance + ((_with->obb.p_0_max.Distance - _with->obb.p_0_min.Distance) / 2);
						float diff = (d1 * Maths::ISgn(dot)) - d2;			
						if (Maths::Abs(diff) < _diff) return true; //Planar are coplanar at a difference in distance of less than 0.6
					}
					return false;
				}

				SurfaceSegment()
					: internalSegGraph(NULL)
					, internalStrGraph(NULL)
					, label("Default")
					, internalGraphSelectionIndex(-1)    //view all internal segments
					, shape_primitive(SegmentsGraph::unknown)
					, bounding_min(1)
					, bounding_max(-1)
					, orientation(0)
					, visibilityCount(0)
					, coverage(0)
					, plane_d(0)
					, maxDecompositionCount(0)
					, points(new std::vector<int>())
					, connections(new std::map<int, EdgeSegment*>())
				{ }

				SurfaceSegment(SurfaceSegment * _copy)
				{
					

				}

				~SurfaceSegment()
				{
					delete connections;					
				}
				
			};

			struct EdgeSegment
			{
				int id;
				int occlusion;	//0 indicates that this edge is visible from all directions used in the training set.
				std::string label;
				std::vector<int> * points;
				std::map<int, std::pair<int, SurfaceSegment*>> * connections;  //pair<strength, *surfacesegment>

				int sizeInBytes()
				{
					int s = 0;
					s += sizeof(int); //id
					s += sizeof(int); //occlusion					
					s += sizeof(int) + (points->size() * sizeof(int)); //number of points + points
					s += sizeof(int) + (connections->size() * sizeof(int) * 2); //segmentid + strength of edge				
					return s;
				}

				EdgeSegment()
					: label("Default")
					, occlusion(0)
					, points(NULL)
					, connections(NULL)
				{ 					
					BOOST_ASSERT(points==NULL);
					points = new std::vector<int>();
					BOOST_ASSERT(points!=NULL);

					BOOST_ASSERT(connections==NULL);
					connections = new std::map<int, std::pair<int, SurfaceSegment*>>();
					BOOST_ASSERT(connections!=NULL);					
				}

				~EdgeSegment()
				{
					delete points;
					delete connections;
				}
				
			};

			std::map<int, SurfaceSegment*> * m_disjointSegmentsGraphs;		

			std::map<int, int> * m_pointsPerSegment;
			std::map<int, int>::iterator m_pointsPerSegmentIterator;

			std::map<int, SurfaceSegment*> * m_surfaceSegments;
			std::map<int, SurfaceSegment*>::iterator m_surfaceSegmentsIterator;			

			std::map<int, EdgeSegment*> * m_edgeSegments;
			std::map<int, EdgeSegment*>::iterator m_edgeSegmentsIterator;

			std::map<int, SurfaceSegment*> * m_prunedSurfaceSegments;			
			std::map<int, EdgeSegment*> * m_prunedEdgeSegments;			

			std::map<int, OBB*> * m_planePrimitivesOBB;
			std::map<int, OBB*>::iterator m_planePrimitivesOBBIterator;

			std::map<int, std::pair<Vector3, std::vector<Vector3>>> * m_ransacSurfacePoints;

			std::map<int, int> * m_surfacePromotions;

			SegmentsGraph(PointCloudSegmentation * p_pcs);			

			//Create an instance of a new Disjoint Graph covering a disjoint part of the point cloud
			//The state returned should be used to store the first segment processed.
			SurfaceSegment * CreateAddDisjointGraph(int p_segmentId);

			//Increase edge strength
			void SetEdgeStrength(int p_edge, int p_strength);

			//Check if node exists depending on node type (surface, edge)
			bool SegmentExists(int p_nodeId, int p_typeId);

			//Check if node exists irrispective on node type (surface, edge)
			bool SegmentExists(int p_nodeId);

			//Check if a state exists
			bool SurfaceSegmentExists(int p_stateId);

			//Check if an edge exists
			bool EdgeSegmentExists(int p_stateId);

			//Create a new surface state in the graph
			void CreateNewSurfaceSegment(int p_id);

			//Create a new surface state for an internal graph. Use count on internal surface segments
			int CreateNewSurfaceSegment();

			//Create a new surface state for an internal graph. Use count on internal surface segments
			int CreateNewSurfaceSegment(SegmentsGraph::Shape p_primitive);

			//Create a new edge state in the graph
			int CreateNewEdgeSegment(int p_id);

			//Adds connections between p_surface and p_edge	
			void AddConnection(SurfaceSegment * p_surface, EdgeSegment * p_edge);

			//NOTE HUGE ASSUMPTION THAT p_node1 is of different type than p_node2
			//Adds connections between p_edge and p_state
			void AddConnection(int p_segment1, int p_segment2);
	
			//*************
			void AddPoints(int p_segmentId, std::vector<int> &p_points);
			void RemovePoints(int p_segmentId, std::vector<int> &p_points);

			void IncrementPointCountInSegment(int p_segmentId);

			int PointCountInSegment(int p_segmentId);
			
			//Prune (move to pruned map) Surfaces Segments which have less than p_min_number_of_points.
			void PruneWeakSurfaceStates(int p_min_number_of_points, std::vector<int> &p_pruned_points);

			//Eliminate Edges states which are only connected to one surface state. They are useless for us.
			void PruneEdgeSegmentsWithSingleConnection();

			//Merges small surface segments (typically less than 10 points) to an edge segment it is connected to.
			//This is done in order to remove insignificant surface segments.
			int MergeSurfaceSegmentsWithEdgeSegments(int p_min_number_of_points);

			//Merge Two Surface Segments together (i.e. add all points in fromId to itId
			void MergeSurfaceSegments(int _sourceId, int _sinkId);

			int SurfaceSegmentsCount();

			SurfaceSegment * SurfaceSegmentAtIndex(int p_index);

			SurfaceSegment * SurfaceSegmentWithId(int p_id);

			EdgeSegment * EdgeSegmentWithId(int p_id);

			// Returns a list of indices in breadth first order
			std::vector<int> BreadthFirstTraversal(SurfaceSegment * p_initialState);

			// Returns a list of indices of edge segments connected to surface segments in p_segs
			std::vector<int> GetConnectedEdgeSegments(std::vector<int> p_segs);

			//Returns edge segments partially or fully within a specified sphere
			std::vector<int> GetEdgeSegmentsWithinSphere(Vector3 p_centre, float p_radius);

			OBB ComputeOBB(int _segmentId, float _buffer);

			OBB ComputeOBB(std::vector<int> _segmentIds, float _buffer);

			bool InOBB(SurfaceSegment* _segment_container, SurfaceSegment* _segment_tocheck);
			bool InOBB(SurfaceSegment* _segment_container, Vector3 _position);  //Check Point for OBB Containment
			bool InOBB(SegmentsGraph::OBB _segment_obb, Vector3 _position);

			//Returns a percentage of the points in this segments which fall in the OBB
			float InOBBPercentage(SegmentsGraph::OBB p_obb, SegmentsGraph::SurfaceSegment * p_segment);
			float InOBBPercentage(SegmentsGraph::OBB p_obb, SegmentsGraph::EdgeSegment * p_segment);
			float InOBBPercentageQuick(SegmentsGraph::OBB p_obb, SegmentsGraph::SurfaceSegment * p_segment);
			float InOBBPercentage(SegmentsGraph::OBB p_obb, std::vector<SegmentsGraph::SurfaceSegment *> p_segments); //Used to check segments from merged nodes together.

			void WriteGraphToFile(std::string p_filepath);

			//**********************************************
			//**********Serialisation Functions********************
			void WriteToBSEGFile(std::string _filepath);
			bool ReadFromBSEGFile(std::string _filepath); 
		};
	}
}